/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-01-15     Wangjiangbo       the first version
 */
#include <app/app_motorcontrol.h>
#include <App/app_io.h>
#include <mod/mod_cia402dev.h>
#include <mod/mod_canopen.h>

#define DBG_TAG "mc"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define RPM_TO_HEX      (1666.666f)                    //10.0 / 60.0 * 10000.0;

#define LEFT_INDEX      (LEFT_ID - 1)
#define RIGHT_INDEX     (RIGHT_ID - 1)

extern int force_status;

enum RUN_STATE {
    TO_MC_DISABLE = 0,
    MC_DISABLE,
    TO_MC_ENABLE,
    MC_ENABLE,
};

enum RUN_CMD {
    CMD_NONE = 0,
    CMD_ENABLE = 1,
    CMD_DISABLE,
    CMD_SET_VEL,
    CMD_SET_VEL_LEFT,
    CMD_SET_VEL_RIGHT,
    CMD_SET_VEL_LIFT,
};

const static int8_t motor_config_dir[2] = { -1, 1,};

/* 邮箱控制块 */
static struct rt_mailbox mb;
/* 用于放邮件的内存池 */
static char mb_pool[128];



static void __all_enable(void)
{
    mod_canopen_start_node(0);      //启动所有节点

    for(int i = 0; i < 3; i++)
    {
        cia402_set_vel(i + 1, 0);
        cia402_set_mode(i + 1,   CIA402_VEL_MODE);
        cia402_disable(i + 1);      //06
        cia402_switch_on(i + 1);    //07
    }
    rt_thread_mdelay(100);

    for(int i = 0; i < 3; i++)
    {
        cia402_enable(i + 1);
    }
    rt_thread_mdelay(100);
}

static void __all_disable(void)
{
    for(int i = 0; i < 3; i++)
    {
        cia402_disable(i + 1);
    }
    rt_thread_mdelay(100);
    LOG_D("电机失能");
}

static int32_t __msg_recv(int32_t* p_msg, int32_t* p_paramter, int32_t timeout)
{
    int32_t msg = 0;
    int32_t result = 0;
    result = rt_mb_recv(&mb, (rt_ubase_t *)&msg, timeout);
    *p_msg = msg & 0xffff;
    *p_paramter = (msg >> 16) & 0xffff;
    return result;
}

// 线程入口
static void thread_entry(void* parameter)
{
    int32_t motor_cmd = 0;
    int32_t motor_param = 0;
    int8_t state = TO_MC_DISABLE;

    rt_thread_mdelay(3000);

    mod_canopen_start_node(0);      //启动所有节点

    while(1)
    {
        switch(state)
        {
        case TO_MC_DISABLE:
            __all_disable();
            state = MC_DISABLE;
            break;

        case MC_DISABLE:

            __msg_recv(&motor_cmd, &motor_param, RT_WAITING_FOREVER);

            if(motor_cmd == CMD_ENABLE)
                state = TO_MC_ENABLE;
            break;

        case TO_MC_ENABLE:

            __all_enable();
            state = MC_ENABLE;

            LOG_D("电机使能");
            break;

        case MC_ENABLE:
        {
            int32_t r = __msg_recv(&motor_cmd, &motor_param, 100);
            int16_t motor_vel = motor_param & 0x00ffff;
            if(r == RT_EOK)
            {
                if(motor_cmd == CMD_DISABLE)
                    state = TO_MC_DISABLE;

                if(motor_cmd == CMD_SET_VEL_LEFT)
                    cia402_set_vel(LEFT_ID, (int32_t)(motor_vel * RPM_TO_HEX *motor_config_dir[LEFT_ID-1]));
                else if(motor_cmd == CMD_SET_VEL_RIGHT)
                    cia402_set_vel(RIGHT_ID, (int32_t)(motor_vel * RPM_TO_HEX *motor_config_dir[RIGHT_ID-1]));
                else if(motor_cmd == CMD_SET_VEL_LIFT)
                    cia402_set_vel(LIFT_ID, (int32_t)(motor_vel * RPM_TO_HEX));
            }
            if( r == -RT_ETIMEOUT)
            {
                if(!force_status)
                {
                    cia402_set_vel(LEFT_ID, 0);
                    cia402_set_vel(RIGHT_ID, 0);
                    cia402_set_vel(LIFT_ID, 0);
                }
            }
            if(motor_cmd == CMD_DISABLE)
                state = TO_MC_DISABLE;

            break;
        }
        default:
           break;
        }

    }
}

void app_mc_init(void)
{
#define THREAD_STACK_SIZE   1024
#define THREAD_PRIORITY     7
#define THREAD_TIMESLICE    10

    rt_thread_t tid = RT_NULL;

    rt_err_t result;

    /* 初始化一个 mailbox */
    result = rt_mb_init(&mb,
                        "mbt",                      /* 名称是 mbt */
                        &mb_pool[0],                /* 邮箱用到的内存池是 mb_pool */
                        sizeof(mb_pool) / 4,        /* 邮箱中的邮件数目，因为一封邮件占 4 字节 */
                        RT_IPC_FLAG_FIFO);          /* 采用 FIFO 方式进行线程等待 */
    // 创建线程
    tid = rt_thread_create("motor control",
                            thread_entry, RT_NULL,
                            THREAD_STACK_SIZE,
                            THREAD_PRIORITY, THREAD_TIMESLICE);



    if (tid != RT_NULL)
        rt_thread_startup(tid);

}

void app_mc_enable(void)
{
    rt_err_t result;
    result = rt_mb_send(&mb, CMD_ENABLE);
    LOG_D("请求电机使能");
}

void app_mc_disable(void)
{
    rt_err_t result;
    result = rt_mb_send(&mb, CMD_DISABLE);
    LOG_D("请求电机失能");
}

void app_mc_run(int id, int32_t vel)
{
    rt_err_t result;
    if(id == LEFT_ID)
    {
        int32_t val = ((vel << 16) | CMD_SET_VEL_LEFT);
        result = rt_mb_send(&mb, val);
    }
    else if (id == RIGHT_ID)
    {
        int32_t val = ((vel << 16) | CMD_SET_VEL_RIGHT);
        result = rt_mb_send(&mb, val);
    }
    else if (id == LIFT_ID)
    {
        int32_t val = ((vel << 16) | CMD_SET_VEL_LIFT);
        result = rt_mb_send(&mb, val);
    }
}


